#ifndef BTL_SLAM_VISUALODOMETER
#define BTL_SLAM_VISUALODOMETER

// ###
// ###  WARNING
// ###
// ###    This file is temporary.
// ###    It does not match the expected public API for BTL 2.
// ###

#include <btl/Image/Image.hpp>
#include <btl/Image/ImagePyramid.hpp>
#include <btl/Image/PixelTypes.hpp>
#include <btl/Camera/CameraModel.hpp>

#include <btl/Features/FeatureDescriptor.hpp>
#include <btl/Features/FeatureMatcher.hpp>

#include <Eigen/Dense>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/optional.hpp>

namespace cv
{
class FeatureDetector;
}

namespace btl
{
namespace slam
{

class OdoFrame
{
   public:
      static const int NUM_IMAGE_LEVELS = 2;

      OdoFrame();
      OdoFrame(const btl::ImageRegionConstGrey& frame, int idx);

      void reset(const btl::ImageRegionConstGrey& frame, int idx);
      void reset(const btl::ImageRegionConstGrey& frame);

      btl::ImagePyramidGrey levels;
      int idx;
};

class OdoPatch
{
   public:
      static const int PATCH_SIZE = 9;

      OdoPatch();
      OdoPatch(
         const btl::ImageRegionConstGrey& frame,
         Eigen::Vector2i pos, int level, int frameIdx, float score);

      typedef btl::PatchFeatureDescriptor<btl::PixelGrey, PATCH_SIZE, PATCH_SIZE> PatchDescriptor;
      PatchDescriptor patch;

      // where did this patch come from
      Eigen::Vector2i pos;
      int level;
      int frameIdx;

      // what's the feature score?
      float score;

      // interface used by btl::features algorithms
      typedef PatchDescriptor feature_descriptor_type;
      Eigen::Vector2f position() const { return pos.cast<float>(); }
      const feature_descriptor_type& descriptor() const { return patch; }
};

class OdoTrace
{
   public:
      static int NextId;

      OdoTrace();

      const int id;

      // first time this feature was picked
      OdoPatch first;

      // last matching patch
      // (use first.frameIdx and lastMatch.frameIdx to check the status of matches)
      OdoPatch lastMatch;

      /// Current best estimate of the world position of this landmark
      Eigen::Vector3d bestEstimate;

      // some statistics
      int matchCount;   ///< Number of times this trace was matched
      int inlierCount;  ///< Number of times RANSAC determined this trace to be an inlier
      int outlierCount; ///< Number of times RANSAC determined this trace to be an outlier

      // pair of (frame index, feature image position)
      typedef std::pair<int, Eigen::Vector2f> Measurement;
      std::vector<Measurement> patchTrack;

      // interface used by btl::features algorithms
      typedef OdoPatch::PatchDescriptor feature_descriptor_type;
      Eigen::Vector2f position() const { return lastMatch.pos.cast<float>(); }
      const feature_descriptor_type& descriptor() const { return first.patch; }
};

class OdoKeyFrame
{
   public:
      // Trace id, plus image position
      typedef std::pair<int, Eigen::Vector2f> Measurement;

      OdoKeyFrame();

      OdoFrame frame;
      std::vector<Measurement> measurements;

      Eigen::Affine3d pose;
      bool poseFixed;
      bool hasPoseEstimate;
};

/// A simple visual odometer.
/// In the style of Nister et al., Visual Odometry (CVPR 2004)
class VisualOdometer
{
   public:
      VisualOdometer();
      ~VisualOdometer();

      void setCameraModel(std::auto_ptr<btl::CameraModel>& model);
      void process(const btl::ImageRegionConstGrey& im);

      // ~~~ per frame data
      OdoFrame frame;
      std::vector<OdoPatch> features[OdoFrame::NUM_IMAGE_LEVELS];

      // ~~~ persistent data
      boost::ptr_vector<OdoTrace> traces;
      boost::ptr_vector<OdoKeyFrame> keyframes;

      // pose history
      std::vector<Eigen::Affine3d> poses;
   private:
      void trackFeatures();
      void addKeyFrame();
      void threePointPose();
      void fivePointPose();
      void refinePose();
      void triangulateLandmarks();
      void matchScale();

      void detectFeatures();
      void extendTraces();
      void extendTraces(
         const btl::ImageRegionConstGrey& im, int level,
         std::vector<OdoPatch>::const_iterator fbegin,
         std::vector<OdoPatch>::const_iterator fend,
         boost::ptr_vector<OdoTrace>::iterator tbegin,
         boost::ptr_vector<OdoTrace>::iterator tend);
      void dropDeadTraces();
      void addNewTraces();
      void addNewTraces(
         const btl::ImageRegionConstGrey& im,
         int level,
         std::vector<OdoPatch>::const_iterator fbegin,
         std::vector<OdoPatch>::const_iterator fend,
         size_t tbegin, size_t tend);

      int _keyframeCountdown;
      int _rebuildCountdown;

      boost::scoped_ptr<btl::CameraModel> _pCameraModel;
      boost::scoped_ptr<cv::FeatureDetector> _pDetector;
};
} // namespace slam
} // namespace btl

namespace btl
{
using slam::VisualOdometer;
}

#endif
